#ifndef NDT_MATCHING_H
#define NDT_MATCHING_H
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>               //NDT(正态分布)配准类头文件
#include <pcl/filters/approximate_voxel_grid.h> //滤波类头文件  （使用体素网格过滤器处理的效果比较好）
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/voxel_grid.h>

#include <pcl/registration/ia_ransac.h>
#include <pcl/features/fpfh.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ndt.h>
#include <time.h>

// 定义结构体用于存储NDT和初始变换
struct Trans_between_robots
{
	// 定义变换矩阵
	Eigen::Matrix4f transform_matrix_B2A;
	Eigen::Matrix4f transform_matrix_C2A;
};


struct Frame_first{
    	Frame_first()
	{
		cloud_xyz_i_first_a = pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>);
		cloud_xyz_i_first_b = pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>);
		cloud_xyz_i_first_c = pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>);
	}

	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_xyz_i_first_a;
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_xyz_i_first_b;
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_xyz_i_first_c;

};


Eigen::Matrix4f init_guess_Get(const int& id);
Eigen::Matrix4f NDTMatching_M(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_source,
                             const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_target,Eigen::Matrix4f init_guess);
void adjustTransformation(Eigen::Matrix4f &transform_matrix);






#endif // NDT_MATCHING_H
